#include "pid.h"
#include "usart.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "main.h"

void PID_Init(PID_Controller *pid)
{
	pid->Targetangle = 0.0f;
  pid->Realangle = 0.0f;
	pid->error = 0.0f;
	pid->error_last = 0.0f;
	pid->integral = 0.0f;
	pid->kp = 8.0f;
	pid->ki = 0.1f; 
	pid->kd =0.4f; 
  pid->integralLimit = 800.0f;
  pid->outputLimit = 700.0f;
}

float PID_Update(PID_Controller *pid,float targetangle,float realangle)
{
	int index = 0;//用于判断误差积分是否有效
	pid->Targetangle = targetangle;
  pid->Realangle = realangle;
	pid->error = pid->Targetangle - pid->Realangle ;
  
  //积分分离
	if(fabs(pid->error) > 5.0f) 
	{
		index = 0;
	}
	else
	{
		index = 1;
		pid->integral += pid->error ;
    
    //积分限幅
		if(pid->integral > pid->integralLimit) 
    {
        pid->integral = pid->integralLimit;
    } 
    else if(pid->integral < -pid->integralLimit) 
    {
        pid->integral = -pid->integralLimit;
    }
	}
  
	pid->output = pid->kp * pid->error + index * pid->ki * pid->integral + pid->kd * (pid->error-pid->error_last);
	
  if(pid->output >= pid->outputLimit) 
  {
      pid->output = pid->outputLimit;
  }
  else if(pid->output <= -pid->outputLimit) 
  {
      pid->output = -pid->outputLimit;
  }
  
  pid->error_last  = pid->error;
  
	return pid->output;
}
